/**
* @file mpu6050.c
* @brief Implementation file for the MPU6050 sensor module.
*
* This file contains the implementation of functions for interfacing with the MPU6050 sensor.
* It includes functions for initialization, data reading, and processing sensor data.
* It also defines the I2C pins and other related constants.
* @author Maker Studio
* @license Apache 2.0
* This code is open-sourced under the Apache 2.0 license. When using open-source libraries
* or code, please ensure to acknowledge the original authors and sources.
**/

#include "../inc/mpu6050.h"

int retval;

angle_t current_angle = {0.0f, 0.0f, 0.0f}; 
uint32_t baudrate = I2C_SET_BAUDRATE;
uint8_t hscode = I2C_MASTER_ADDR;

int16_t AccX, AccY, AccZ, GyroX, GyroY, GyroZ;

typedef uint32_t errcode_t;

#define PIN_PULL_TYPE_STRONG_UP 2
#define GPIO_LEVEL_HIGH 1


static int shanghui, xiahui, zuoxuan, youxuan = 0; 

static int Gesture_flag = 0; 

typedef struct
{
    float acc_x, acc_y, acc_z;
    float gyro_x, gyro_y, gyro_z;
} SensorData;


SensorData standardMotion1 = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
SensorData Motion1 = {
    -0.09f, 
    0.04f,  
    -0.09f, 
    18.91f, 
    49.59f, 
    -24.56f 
};
SensorData Motion2 = {
    0.01f,  
    0.03f,  
    0.13f,  
    -1.49f, 
    -6.54f, 
    10.01f  
};

SensorData Motion3 = {
    -0.00f,  
    0.01f,   
    0.12f,   
    10.43f,  
    125.19f, 
    13.89f   
};

SensorData Motion4 = {
    -0.04f, 
    0.03f,  
    0.13f,  
    0.24f,  
    90.40f, 
    -11.87f 
};

// Function prototypes
void MPU6050_Init(void);

int Gesture_Recognition(void);

void MPU6050_GetData(SensorData *data);

void MPU6050_Shoushi_init(void);

void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data);

uint8_t MPU6050_ReadReg(uint8_t RegAddress);

void kalman_filter_init(kalman_filter_t *kf, float q, float r, float x_init, float p_init);

float kalman_filter_update(kalman_filter_t *kf, float measurement);

float calculateEuclideanDistance(SensorData current, SensorData reference);

int isGestureMatched(SensorData current, SensorData reference, float threshold);

void readSensorData(SensorData *data); 

#define NUM_FEATURES 6             
#define NUM_SAMPLES_PER_GESTURE 40 
#define THRESHOLD 150              

void readSensorData(SensorData *data)
{
    MPU6050_GetData(data);
}

void calculate_accel_angle(int16_t AccX, int16_t AccY, int16_t AccZ)
{
    current_angle.roll = atan2(AccX, sqrt(AccY * AccY + AccZ * AccZ)) * 180.0f / M_PI;
    current_angle.pitch = atan2(-AccY, AccZ) * 180.0f / M_PI;
}

void update_gyro_angle(float GyroX, float GyroY, float GyroZ, float dt)
{
    current_angle.pitch += GyroX * dt*100;
    current_angle.roll += GyroY * dt*100;
    current_angle.yaw += GyroZ * dt*100;
}

float calculateEuclideanDistance(SensorData current, SensorData reference)
{
    float sum = pow((current.acc_x - reference.acc_x), 2) +
                pow((current.acc_y - reference.acc_y), 2) +
                pow((current.acc_z - reference.acc_z), 2) +
                pow((current.gyro_x - reference.gyro_x), 2) +
                pow((current.gyro_y - reference.gyro_y), 2) +
                pow((current.gyro_z - reference.gyro_z), 2);
    return sqrt(sum);
}

int isGestureMatched(SensorData current, SensorData reference, float threshold)
{
    (void)threshold;
    float distance = calculateEuclideanDistance(current, reference);
    int dis = (int)distance;
    return dis;
}

void averageGestureSamples(SensorData samples[], int numSamples, SensorData *average)
{
    memset(average, 0, sizeof(SensorData));
    for (int i = 0; i < numSamples; i++)
    {
        *average = (SensorData){
            .acc_x = average->acc_x + samples[i].acc_x,
            .acc_y = average->acc_y + samples[i].acc_y,
            .acc_z = average->acc_z + samples[i].acc_z,
            .gyro_x = average->gyro_x + samples[i].gyro_x,
            .gyro_y = average->gyro_y + samples[i].gyro_y,
            .gyro_z = average->gyro_z + samples[i].gyro_z};
    }
    average->acc_x /= numSamples;
    average->acc_y /= numSamples;
    average->acc_z /= numSamples;
    average->gyro_x /= numSamples;
    average->gyro_y /= numSamples;
    average->gyro_z /= numSamples;
}

/**
 * @brief Initialize the Kalman filter.
 * @param kf Pointer to the Kalman filter structure.
 * @param q Process noise covariance.
 * @param r Measurement noise covariance.
 * @param x_init Initial value of the state.
 * @param p_init Initial value of the estimation error covariance.
 */
void kalman_filter_init(kalman_filter_t *kf, float q, float r, float x_init, float p_init)
{
    kf->Q = q;
    kf->R = r;
    kf->P = p_init;
    kf->K = 0;
    kf->X = x_init;
}

/**
 * @brief Update the Kalman filter with a new measurement.
 * @param kf Pointer to the Kalman filter structure.
 * @param measurement New measurement value.
 * @return Updated state value.
 */
float kalman_filter_update(kalman_filter_t *kf, float measurement)
{
    float innovation = measurement - kf->X;
    kf->K = kf->P / (kf->P + kf->R);
    kf->X += kf->K * innovation;
    kf->P = (1 - kf->K) * kf->P + kf->Q;
    return kf->X;
}

/**
 * @brief Write a value to a register of the MPU6050 sensor.
 * @param RegAddress Register address.
 * @param Data Data to write.
 */
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
    i2c_data_t mpudata = {0};
    uint8_t databuff[2] = {RegAddress, Data};
    mpudata.send_buf = databuff;
    mpudata.send_len = sizeof(databuff);
    retval = IoTI2cWrite(CONFIG_I2C_MASTER_BUS_ID,
                                  MPU6050_ADDRESS,
                                  mpudata.send_buf,
                                  mpudata.send_len);
    if (retval != IOT_SUCCESS)
    {
        osal_printk("MPU6050_WriteReg读取失败！！！！！！！！！！！/r/n");
        osal_printk("i2c%d master send error %x !\r\n", CONFIG_I2C_MASTER_BUS_ID, retval);
    }
}

/**
 * @brief Read a value from a register of the MPU6050 sensor.
 * @param RegAddress Register address.
 * @return Data read from the register.
 */
uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
    i2c_data_t mpudata1 = {0};
    uint8_t databuff[2] = {0};
    mpudata1.send_buf = &RegAddress;
    mpudata1.send_len = sizeof(RegAddress);
    mpudata1.receive_buf = databuff;
    mpudata1.receive_len = sizeof(databuff);
    retval = uapi_i2c_master_writeread(CONFIG_I2C_MASTER_BUS_ID,MPU6050_ADDRESS,&mpudata1);
    if (retval != IOT_SUCCESS)
    {
        osal_printk("MPU6050_ReadReg读取失败！！！！！！！！！！！/r/n");
        osal_printk("I2cRead() failed, %0X!\n", retval);
    }
    return databuff[0];
}

/**
 * @brief Initialize the MPU6050 sensor.
 */
void MPU6050_Init(void)
{
    uapi_i2c_deinit(CONFIG_I2C_MASTER_BUS_ID);
    osal_printk("mpu init start!!!\r\n");

    IoTGpioInit(IOT_IO_NAME_GPIO_15);
    IoTGpioInit(IOT_IO_NAME_GPIO_16);

    IoSetFunc(IOT_IO_NAME_GPIO_15, 2);
    IoSetFunc(IOT_IO_NAME_GPIO_16, 2);
    IoTI2cInit(1, I2C_SET_BAUDRATE);
    MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0X80);
    osDelay(2);
    MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0X00);
    MPU6050_WriteReg(MPU6050_RA_INT_ENABLE, 0X00);
    MPU6050_WriteReg(MPU6050_RA_USER_CTRL, 0X00);
    MPU6050_WriteReg(MPU6050_RA_FIFO_EN, 0X00);
    MPU6050_WriteReg(MPU6050_RA_INT_PIN_CFG, 0X80);
    MPU6050_WriteReg(MPU6050_RA_CONFIG, 0x04);
    MPU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG, 0x1C);
    MPU6050_WriteReg(MPU6050_RA_INT_PIN_CFG, 0X1C);
    MPU6050_WriteReg(MPU6050_RA_INT_ENABLE, 0x40);
}

/**
 * @brief Get data from the MPU6050 sensor.
 * @param data Pointer to the SensorData structure to store the data.
 */
void MPU6050_GetData(SensorData *data)
{
    uint8_t DataH, DataL;

    kalman_filter_t kf_acc_x, kf_acc_y, kf_acc_z;
    kalman_filter_t kf_gyro_x, kf_gyro_y, kf_gyro_z;

    kalman_filter_init(&kf_acc_x, 0.001, 0.03, 0.0, 1.0);
    kalman_filter_init(&kf_acc_y, 0.001, 0.03, 0.0, 1.0);
    kalman_filter_init(&kf_acc_z, 0.001, 0.03, 0.0, 1.0);
    kalman_filter_init(&kf_gyro_x, 0.001, 0.03, 0.0, 1.0);
    kalman_filter_init(&kf_gyro_y, 0.001, 0.03, 0.0, 1.0);
    kalman_filter_init(&kf_gyro_z, 0.001, 0.03, 0.0, 1.0);

    DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H);
    DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);
    AccX = (DataH << 8) | DataL;
    data->acc_x = kalman_filter_update(&kf_acc_x, AccX / 16384.0f);

    DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H);
    DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L);
    AccY = (DataH << 8) | DataL;
    data->acc_y = kalman_filter_update(&kf_acc_y, AccY / 16384.0f);

    DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H);
    DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L);
    AccZ = (DataH << 8) | DataL;
    data->acc_z = kalman_filter_update(&kf_acc_z, AccZ / 16384.0f);

    DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H);
    DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L);
    GyroX = (DataH << 8) | DataL;
    data->gyro_x = kalman_filter_update(&kf_gyro_x, GyroX / 131.0f);

    DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H);
    DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L);
    GyroY = (DataH << 8) | DataL;
    data->gyro_y = kalman_filter_update(&kf_gyro_y, GyroY / 131.0f);

    DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);
    DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);
    GyroZ = (DataH << 8) | DataL;
    data->gyro_z = kalman_filter_update(&kf_gyro_z, GyroZ / 131.0f);
}

/**
 * @brief Recognize high-level gestures.
 * @return Gesture flag indicating the recognized gesture.
 */
int Gesture_Recognition_High(void)
{
    SensorData *motion1Samples = (SensorData *)malloc(NUM_SAMPLES_PER_GESTURE * sizeof(SensorData));
    if (motion1Samples == NULL)
    {
        osal_printk("Memory allocation failed for motion1Samples\n");
        return -1;
    }

    for (int i = 0; i < NUM_SAMPLES_PER_GESTURE; ++i)
    {
        readSensorData(&motion1Samples[i]);
    }

    averageGestureSamples(motion1Samples, NUM_SAMPLES_PER_GESTURE, &standardMotion1);
    int flag1 = isGestureMatched(standardMotion1, Motion1, THRESHOLD);
    int flag2 = isGestureMatched(standardMotion1, Motion2, THRESHOLD);
    int flag3 = isGestureMatched(standardMotion1, Motion3, THRESHOLD);
    int flag4 = isGestureMatched(standardMotion1, Motion4, THRESHOLD);

    if (flag1 >= 155 && flag1 <= 190)
    {
        osal_printk("Detected Motion 1\n");
        osal_printk("迪迦變身\n");
        Gesture_flag = 1;
    }
    else if (flag2 >= 75 && flag2 <= 115)
    {
        osal_printk("Detected Motion 2\n");
        Gesture_flag = 2;
        osal_printk("L动作\n");
    }
    else if (flag3 >= 150 && flag3 <= 185)
    {
        osal_printk("Detected Motion 3\n");
        Gesture_flag = 3;
        osal_printk("竖直向下动作\n");
    }
    else if (flag4 >= 100 && flag4 <= 140)
    {
        osal_printk("Detected Motion 4\n");
        osal_printk("逆时针圈圈\n");
        Gesture_flag = 4;
    }

    free(motion1Samples);

    return Gesture_flag;
}

/**
 * @brief Recognize low-level gestures.
 * @return Gesture flag indicating the recognized gesture.
 */
int Gesture_Recognition_Low(void)
{
    SensorData *motion1Samples = (SensorData *)malloc(NUM_SAMPLES_PER_GESTURE * sizeof(SensorData));
    if (motion1Samples == NULL)
    {
        osal_printk("Memory allocation failed for motion1Samples\n");
        return -1;
    }

    for (size_t i = 0; i < 30; i++)
    {
        readSensorData(motion1Samples);
        calculate_accel_angle(motion1Samples->acc_x, motion1Samples->acc_y, motion1Samples->acc_z);
        float dt = I2C_TASK_DURATION_MS / 1000.0f;
        update_gyro_angle(motion1Samples->gyro_x, motion1Samples->gyro_y, motion1Samples->gyro_z, dt);

        if (current_angle.pitch > 30)
        {
            shanghui++;
            if (shanghui > 5)
            {
                osal_printk(" shang hui OK\r\n");
                shanghui = 0;
                Gesture_flag = 5;
            }
        }
        else if (current_angle.pitch < -10)
        {
            xiahui++;
            if (xiahui > 5)
            {
                osal_printk(" xia hui OK\r\n");
                xiahui = 0;
                Gesture_flag = 6;
            }
        }
        else if (current_angle.roll < -20)
        {
            zuoxuan++;
            if (zuoxuan > 5)
            {
                osal_printk(" zuo xuan OK\r\n");
                zuoxuan = 0;
                Gesture_flag = 7;
            }
        }
        else if (current_angle.roll > 30)
        {
            youxuan++;
            if (youxuan > 5)
            {
                osal_printk(" you xuan OK\r\n");
                youxuan = 0;
                Gesture_flag = 8;
            }
        }
        else
        {
            osal_printk("No known gesture detected\n");
            Gesture_flag = 0;
        }
    }

    free(motion1Samples);
    return Gesture_flag;
}

